#include <Arduino.h>
#include <Servo.h>
#include "Leg.cpp"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>舵机
Servo Servo1,Servo2,Servo3,Servo4;

int Engine1=14,Engine2=16,Engine3=12,Engine4=13;
int ServoSpeed=300;
Leg RF(Servo1,false),RB(Servo2,false),LF(Servo3),LB(Servo4);

void MotionInit() {
  Servo1.attach(Engine1,500,2500);
  Servo2.attach(Engine2,500,2500);
  Servo3.attach(Engine3,500,2500);
  Servo4.attach(Engine4,500,2500);
  Servo1.write(90); 
  Servo2.write(90);
  Servo3.write(90);
  Servo4.write(90);
}
void Front() {
  LF.front();
  RB.front();
  delay(100);
  RF.back();
  LB.back();
  delay(100);
  LF.recover();
  RB.recover();
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  RF.front();
  LB.front();
  delay(100);
  LF.back();
  RB.back();
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  LF.recover();
  RB.recover();
}
void Back() {
  LF.back();
  RB.back();
  delay(100);
  RF.front();
  LB.front();
  delay(100);
  LF.recover();
  RB.recover();
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  RF.back();
  LB.back();
  delay(100);
  LF.front();
  RB.front();
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  LF.recover();
  RB.recover();
}
void Left() {
  RF.setAngle(100);
  LB.setAngle(100);
  delay(100);
  LF.setAngle(60);
  RB.setAngle(60);
  delay(100);
  RF.setAngle(140);
  LB.setAngle(140);
  delay(100);
  LF.setAngle(40);
  RB.setAngle(40);
  delay(100);
  RF.recover();
  LB.recover();
  LF.recover();
  RB.recover();
  delay(100);

  RF.setAngle(80);
  LB.setAngle(80);
  delay(100);
  LF.setAngle(120);
  RB.setAngle(120);
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  LF.setAngle(140);
  RB.setAngle(140);
  delay(100);
  LF.recover();
  RB.recover();
  delay(100);
}
void Right() {
  RF.setAngle(80);
  LB.setAngle(80);
  delay(100);
  LF.setAngle(120);
  RB.setAngle(120);
  delay(100);
  RF.setAngle(40);
  LB.setAngle(40);
  delay(100);
  LF.setAngle(140);
  RB.setAngle(140);
  delay(100);
  RF.recover();
  LB.recover();
  LF.recover();
  RB.recover();
  delay(100);

  RF.setAngle(100);
  LB.setAngle(100);
  delay(100);
  LF.setAngle(60);
  RB.setAngle(60);
  delay(100);
  RF.recover();
  LB.recover();
  delay(100);
  LF.setAngle(40);
  RB.setAngle(40);
  delay(100);
  LF.recover();
  RB.recover();
  delay(100);
}
void SitDown() {
  LB.front();
  RB.front();
  delay(3000);
  LB.recover();
  RB.recover();
  delay(100);
}
void LieDown() {
  RF.turnTo(90);
  LF.turnTo(90);
  RB.turnTo(-90);
  LB.turnTo(-90);
}
void RiseLeftHand() {
  LF.turnTo(90);
  delay(100);
  LF.turnTo(60);
  delay(100);
}
void RiseRightHand() {
  RF.turnTo(90);
  delay(100);
  RF.turnTo(60);
  delay(100);
}
void DoSleep(){
  RF.turnTo(-90);
  LF.turnTo(-90);
  RB.turnTo(90);
  LB.turnTo(90);
}